// --------------------------------------------------------------------------------------------------------------------
// <copyright file="FindIKWithGoodElbowClearanceFromBaseXYPlane.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.Runtime.Arm
{
    using System;
    using System.Collections.Generic;

    using Microsoft.Robotics.Collision;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;

    using ShoNS.Array;

    /// <summary>
    /// A generic class that finds a good joint configuration such that the elbow is high without collision with the table
    /// </summary>
    public class FindIKWithGoodElbowClearanceFromBaseXYPlane
    {
        /// <summary>
        /// Translational threshold to decide whether a linear motion is small
        /// </summary>
        private static double translationalThreshold = 0.02;

        /// <summary>
        /// Rotational threshold to decide a rotational motion is small
        /// </summary>
        private static double rotationalThreshold = 5.0 * MathConstants.Degrees2Radians;

        /// <summary>
        /// Trace context for this class
        /// </summary>
        private static TraceContext elbowPlannerContext = TraceContexts.FromString(TraceContexts.Manip, "ElbowHeightPlanner");

        /// <summary>
        /// Finds a good joint configuration with good elbow clearance.
        /// The process is as follows:
        /// 1. Based on default seeds passed in as an argument, calculate IK solutions
        /// 2. If argument numRandomSeeds is not zero, we also calculate IK for random seeds
        /// 3. Based on the IK solutions we collected above, we check them and get those that
        ///    are above a certain height threshold.  The lower height is based on cut-off percentage
        ///    that is passed in and the largest height: lower = largest_height * cut_off_percentage
        /// 4. For those that are above a certain height threshold, we then rank them based on their
        ///    closeness to the current joint configuration
        /// 5. For each of these sorted joint configurations, we generate a joint trajectory through linear
        ///    interpolation, which is what joint position controller does at present.  This trajectory
        ///    represents what the arm should look like while the joint position controller is executing
        ///    the given target joint configuration.  We also check whether each joint of the arm is above a certain
        ///    height threshold at each interpolated configuration to make sure there is enough clearance as well
        ///    The first joint configuration that satisfies the criterion above is the output of this function
        /// </summary>
        /// <param name="kc">A kinematic chain</param>
        /// <param name="targetPose">Target pose</param>
        /// <param name="currentJoint">Current joint configuration</param>
        /// <param name="defaultSeeds">Default seed joint configurations</param>
        /// <param name="minimumHeightWithoutCollision">Absolute minimum height to make sure any joint is not colliding with the table.
        /// This is used as the rigorous check at the end of the function.  It is not used by the clearance filtering</param>
        /// <param name="cutOffPercentage">Cut off score based on the highest score of the IK solutions in terms of percentage</param>
        /// <param name="jointWeightFactors">Weight factors used to scale joint travel distances for ranking different trajectories</param>
        /// <param name="elbowClearanceCheckList">Checklist for the elbow height</param>
        /// <param name="extender">Equivalent inverse kinematics solution generator</param>
        /// <param name="translationalTolerance">Translational tolerance for IK computation, in meter</param>
        /// <param name="rotationalTolerance">Rotational tolerance for IK computation, in radian</param>
        /// <param name="minimumElbowHeight">Minimum elbow height we want to guarantee</param>
        /// <param name="trajectoryQualityThreshold">A number which indicates the threshold of the quality of a
        /// trajectory that can be considered as a good one.  This parameter is still in experimental stage.</param>
        /// <param name="numRandomSeeds">How many random seeds we want to use for IK, default number is 0 and 0 should
        /// be used if the defaultSeeds that are passed in are expected to be good enough</param>
        /// <param name="numSamplingWaypoints">Number of waypoints to sample for collision checking</param>
        /// <returns>A good joint configuration</returns>
        public static DoubleArray FindIK(
            KinematicChain kc,
            Pose targetPose,
            DoubleArray currentJoint,
            List<DoubleArray> defaultSeeds,
            double minimumHeightWithoutCollision,
            double cutOffPercentage,
            DoubleArray jointWeightFactors,
            bool[] elbowClearanceCheckList,
            InverseKinematicsNumerical.InverseKinematicsExtender extender = null,
            double translationalTolerance = 0.025,
            double rotationalTolerance = 0.005,
            double minimumElbowHeight = 0.2,
            double trajectoryQualityThreshold = 10.0,
            int numRandomSeeds = 0,
            int numSamplingWaypoints = 100)
        {
            TraceOut.Info(elbowPlannerContext, "Plan goal: {0}", targetPose);
            TraceOut.Info(elbowPlannerContext, "current configuration: {0}", currentJoint.Transpose());
            TraceOut.Info(elbowPlannerContext, "numDefaultSeeds: {0}, minimumJointHeight: {1}, minimumElbowHeight: {2}", defaultSeeds.Count, minimumHeightWithoutCollision, minimumElbowHeight);

            // In case there is no solutions found, let's not move anywhere
            DoubleArray goodIK = null;
            Matrix4 targetPoseInMatrixForm = HomogenousTransform.CreateFromPose(targetPose);

            // step 0: compute IK solutions from default seeds
            bool maxIterationExceeded;
            bool jointsWithinRange;
            List<DoubleArray> ikSolutionsFromDefaultSeeds = new List<DoubleArray>();
            for (int i = 0; i < defaultSeeds.Count; ++i)
            {
                DoubleArray solution = InverseKinematicsNumerical.SolveIKNumerical(
                    kc,
                    targetPoseInMatrixForm,
                    defaultSeeds[i],
                    out maxIterationExceeded,
                    out jointsWithinRange,
                    translationalTolerance,
                    rotationalTolerance);

                if (!maxIterationExceeded && jointsWithinRange)
                {
                    if (null != extender)
                    {
                        ikSolutionsFromDefaultSeeds.AddRange(extender(solution));
                    }
                    else
                    {
                        ikSolutionsFromDefaultSeeds.Add(solution);
                    }
                }
            }

            // step 1: compute IK solutions from current joint configuration
            DoubleArray solutionFromCurrentJoint = InverseKinematicsNumerical.SolveIKNumerical(
                kc,
                targetPoseInMatrixForm,
                currentJoint,
                out maxIterationExceeded,
                out jointsWithinRange,
                translationalTolerance,
                rotationalTolerance);

            // step 2: setup new ik solution list
            List<DoubleArray> ikSolutions = new List<DoubleArray>();

            // step 3: we add solutions from the default seeds here
            if (ikSolutionsFromDefaultSeeds.Count > 0)
            {
                ikSolutions.AddRange(ikSolutionsFromDefaultSeeds);
            }

            // step 4: we add the solution from the current joint position here
            if (!maxIterationExceeded && jointsWithinRange)
            {
                if (null != extender)
                {
                    ikSolutions.AddRange(extender(solutionFromCurrentJoint));
                }
                else
                {
                    ikSolutions.Add(solutionFromCurrentJoint);
                }
            }

            // step 5: generate some new IK solutions based on random seeds if requested
            if (numRandomSeeds > 0)
            {
                ikSolutions.AddRange(InverseKinematicsNumerical.SolveIKNumerical(kc, targetPoseInMatrixForm, numRandomSeeds, translationalTolerance, rotationalTolerance, extender));
            }

            TraceOut.Info(elbowPlannerContext, "IK solutions found before filtering: {0}", ikSolutions.Count);

            if (ikSolutions.Count == 0)
            {
                TraceOut.Warning(elbowPlannerContext, "No IK solutions found");
                return null;
            }

            // step 6: filter out solutions where elbow is below the threshold height at the goal configuration
            double[] scores;
            InverseKinematicsFilter filter = new InverseKinematicsFilterMaximumClearanceFromPlane(kc, elbowClearanceCheckList, Vector3.ZAxis, new Vector3(0, 0, 0));
            List<DoubleArray> sortedSolutions = filter.SortJointConfigurationAndCleanUpDuplicated(ikSolutions, out scores);
            TraceOut.Info(elbowPlannerContext, "number of unique solutions after filtering {0}", sortedSolutions.Count);

            List<DoubleArray> solutionsWithGoodClearance = new List<DoubleArray>();

            // step 6.1: if the top score is smaller than a threshold, we give up (there could be other tricks to add here, but for now let's give up)
            if (Math.Abs(scores[0]) < minimumElbowHeight)
            {
                TraceOut.Warning(elbowPlannerContext, "no good IK solutions found such that the elbow is above specified height: {0}", minimumElbowHeight);

                // explicitly return null indicating that there is no valid IK solution that satisfies safe elbow height
                return null;
            }

            // step 6.2: find the correct lowerbound from the best score
            double lower = scores[0] * cutOffPercentage;
            TraceOut.Info(elbowPlannerContext, "searching for valid trajectory with good elbow clearance: with clearance range {0} ~ {1}", lower, scores[0]);

            // step 6.3: looking for the point to throw away invalid solutions based on elbow clearance
            for (int i = 0; i < sortedSolutions.Count; ++i)
            {
                // scores are the heights of the elbow but negated
                // so we use absolute value
                if (Math.Abs(scores[i]) > Math.Abs(lower))
                {
                    solutionsWithGoodClearance.Add(sortedSolutions[i]);
                    continue;
                }

                // no good solutions, get out of the loop and move to the next step
                break;
            }

            TraceOut.Info(elbowPlannerContext, "number of solutions with good elbow clearance {0}", solutionsWithGoodClearance.Count);

            // step 7: CHECKPOINT - check whether there is any valid solution left such that the elbow height is above the threshold
            if (solutionsWithGoodClearance.Count >= 1)
            {
                // at least one solution has been found
                // before exit, we prepare the result as follows

                // step 8: sort them based on joint change, we prefer smaller joint travel distance
                InverseKinematicsFilter filter2 = new InverseKinematicsFilterWeightedLeastChange(currentJoint, jointWeightFactors);
                List<DoubleArray> sortedSolutionsWithLeastChange = filter2.SortJointConfiguration(solutionsWithGoodClearance, out scores);

                // step 9: final check of the solution to see if the robot arm is having any collision with the table
                for (int i = 0; i < sortedSolutionsWithLeastChange.Count; ++i)
                {
                    bool minimumJointHeightConstraintSatisfied = CheckCubicSplineJointTrajectoryWithMinimumJointHeightFromXYPlaneAndSelfCollision(kc, currentJoint, sortedSolutionsWithLeastChange[i], minimumHeightWithoutCollision, numSamplingWaypoints);

                    if (minimumJointHeightConstraintSatisfied)
                    {
                        TraceOut.Info(elbowPlannerContext, "Index of the first solution that passed collision checking: {0}", i);

                        // Currently, there is no one single number that fits all situations.  We may need to add more mechanisms on a higher-level
                        // to decide what to do when one criterion failed.  Or to pass in this number when motion planner is called.
                        // TODO: The ratio is still not decided yet, and we are really by-passing this checking for most of the cases since we have a big
                        // threshold as used below.
                        if (ComputeEndEffectorTravelDistanceRatio(kc, currentJoint, sortedSolutionsWithLeastChange[i], numSamplingWaypoints) > trajectoryQualityThreshold)
                        {
                            TraceOut.Warning(elbowPlannerContext, "The trajectory seems crazy, refuse to report as a valid solution");
                        }
                        else
                        {
                            goodIK = sortedSolutionsWithLeastChange[i];
                            break;
                        }
                    }

                    TraceOut.Info(elbowPlannerContext, "Index of the first solution that failed collision checking: {0}", i);
                }
            }

            if (null != goodIK)
            {
                TraceOut.Info(elbowPlannerContext, "Solution returned: {0}", goodIK.Transpose());
            }
            else
            {
                TraceOut.Warning(elbowPlannerContext, "No IK solution found and returned");
            }

            // The trajectory returned here could be null, which is its initial value
            // This situation happens when there is no valid target IK found
            return goodIK;
        }

        /// <summary>
        /// Interpolate joint trajectory assuming cubic spline interpolation and check
        /// at discretized waypoints to see if there is any collision by looking at the
        /// height of each joint from the XY plane.  Self-collision is also checked at each waypoint.
        /// Only trajectories along which no joint goes below the minimum height from the XY plane are collision-free.
        /// Caution: assumption is made that 1) the joint position controller only uses the
        /// current joint angle and the target joint angle as the cubic spline, and 2) start
        /// and end joint velocities are both zero.  Because we assume that the velocities are
        /// both zero, we can ignore the actual duration of the trajectory for sampling the
        /// waypoints.
        /// </summary>
        /// <param name="kc">Kinematic chain</param>
        /// <param name="start">Start point</param>
        /// <param name="end">End point</param>
        /// <param name="minimumHeight">Minimum height without collision</param>
        /// <param name="numSamplingWaypoints">Number of waypoints to sample for collision checking</param>
        /// <returns>Whether there is no collision detected</returns>
        private static bool CheckCubicSplineJointTrajectoryWithMinimumJointHeightFromXYPlaneAndSelfCollision(KinematicChain kc, DoubleArray start, DoubleArray end, double minimumHeight, int numSamplingWaypoints)
        {
            double[,] targetPositions = new double[start.size0, 2];
            double[,] targetVelocities = new double[start.size0, 2];
            double[] times = { 0, 1 };

            // assemble cubic spline
            for (int i = 0; i < start.size0; ++i)
            {
                targetPositions[i, 0] = start[i];
                targetPositions[i, 1] = end[i];
            }

            CubicSpline cs = new CubicSpline(
                start.size0,
                2,
                times,
                targetPositions,
                targetVelocities);

            bool selfCollisionDetected = false;

            // check collision, need to make sure the last waypoint is included for checking
            for (int i = 0; i <= numSamplingWaypoints; ++i)
            {
                DoubleArray intermediateConfig = DoubleArray.From(cs.GetPositions((double)i * times[1] / (double)numSamplingWaypoints)).Transpose();

                // check self-collision if it is not detected yet
                // we do not response actively and refuse this trajectory right now since
                // we want to observe how the self-collision checking functions before we
                // utilize it fully
                if (false == selfCollisionDetected)
                {
                    selfCollisionDetected = KinematicChainSelfCollisionChecker.CheckSelfCollision(kc, intermediateConfig);

                    if (true == selfCollisionDetected)
                    {
                        TraceOut.Warning(elbowPlannerContext, "Self-collision detected for start: {0} to end: {1}", start.Transpose(), end.Transpose());
                    }
                }

                // check joint height
                Matrix4[] transfs = kc.GetJointTransforms(intermediateConfig);

                // examine each joint to see if it is colliding
                // this is approximate in the way that we only 
                // check each joint instead of the entire geometry
                // of the robot arm
                for (int j = 0; j < transfs.Length; ++j)
                {
                    // check the z component which is the height
                    if (transfs[j][2, 3] < minimumHeight)
                    {
                        return false;
                    }
                }
            }

            return true;
        }

        /// <summary>
        /// Interpolate joint trajectory assuming cubic spline interpolation and calculate
        /// the travelling distance of the end effector.
        /// </summary>
        /// <param name="kc">Kinematic chain</param>
        /// <param name="start">Start point</param>
        /// <param name="end">End point</param>
        /// <param name="numSamplingWaypoints">Number of waypoints for collision checking</param>
        /// <returns>A ratio that indicates the smoothness of the trajectory</returns>
        private static double ComputeEndEffectorTravelDistanceRatio(KinematicChain kc, DoubleArray start, DoubleArray end, int numSamplingWaypoints)
        {
            Pose startPose = new Pose(kc.GetEndEffectorTransform(start));
            Pose endPose = new Pose(kc.GetEndEffectorTransform(end));
            double shortestTranslationalDistance = (endPose.Position - startPose.Position).Length();
            double shortestRotationalDistance = new Pose(HomogenousTransform.Inverse(HomogenousTransform.CreateFromPose(startPose)) * HomogenousTransform.CreateFromPose(endPose)).GetZYXEulerAngles().Length();

            double translationalDistance = 0;
            double rotationalDistance = 0;
            Pose previousPose = new Pose();
            bool justStarted = true;

            double[,] targetPositions = new double[start.size0, 2];
            double[,] targetVelocities = new double[start.size0, 2];
            double[] times = { 0, 1 };

            for (int i = 0; i < start.size0; ++i)
            {
                targetPositions[i, 0] = start[i];
                targetPositions[i, 1] = end[i];
            }

            // assemble cubic spline
            CubicSpline cs = new CubicSpline(
                start.size0,
                2,
                times,
                targetPositions,
                targetVelocities);

            // accumulate the travelling distances of the end effector
            for (int i = 0; i <= numSamplingWaypoints; ++i)
            {
                DoubleArray intermediateConfig = DoubleArray.From(cs.GetPositions((double)i * times[1] / (double)numSamplingWaypoints)).Transpose();
                Pose currentPose = new Pose(kc.GetEndEffectorTransform(intermediateConfig));
                if (true == justStarted)
                {
                    previousPose = currentPose;
                    justStarted = false;
                    continue;
                }

                // compute the linear distance at this step
                translationalDistance += (previousPose.Position - currentPose.Position).Length();

                // compute the angular distance at this step
                rotationalDistance += Math.Abs(HomogenousTransform.ToAxisAngle(HomogenousTransform.Inverse(HomogenousTransform.CreateFromPose(previousPose)) * HomogenousTransform.CreateFromPose(currentPose)).Angle);
                previousPose = currentPose;
            }

            // Compute trajectory properties
            double translationalDistanceRatio = translationalDistance / shortestTranslationalDistance;
            double rotationalDistanceRatio = rotationalDistance / shortestRotationalDistance;
            double rotationalDistanceOverTranslationalDistance = rotationalDistance / translationalDistance;

            // If the overall translational change for the motion is very small
            // we should ignore the ratio since it can be very numerically unstable
            if (Math.Abs(translationalDistance - shortestTranslationalDistance) < translationalThreshold)
            {
                TraceOut.Info(elbowPlannerContext, "Absolute actual translational difference along trajectory ({0}) is smaller than threshold ({1}) and is set to be the same as the shortest", Math.Abs(translationalDistance - shortestTranslationalDistance), translationalThreshold);
                translationalDistanceRatio = 1.0;
            }

            // If the overall rotational change for the motion is very small
            // we should ignore the ratio since it can be very numerically unstable
            if (Math.Abs(rotationalDistance - shortestRotationalDistance) < rotationalThreshold)
            {
                TraceOut.Info(elbowPlannerContext, "Absolute actual orientation difference along trajectory ({0}) is smaller than threshold ({1}) and is set to be the same as the shortest", Math.Abs(rotationalDistance - shortestRotationalDistance), rotationalThreshold);
                rotationalDistanceRatio = 1.0;
            }

            // Trace out information
            TraceOut.Info(elbowPlannerContext, "Actual translational distance vs. shortest translational distance : {0}, {1}, {2}", translationalDistance, shortestTranslationalDistance, translationalDistanceRatio);
            TraceOut.Info(elbowPlannerContext, "Actual rotational distance vs. shortest rotational distance : {0}, {1}, {2}", rotationalDistance, shortestRotationalDistance, rotationalDistanceRatio);
            TraceOut.Info(elbowPlannerContext, "Actual rotational distance over translational distance : {0}", rotationalDistanceOverTranslationalDistance);

            double combined = 0.5 * (translationalDistance / shortestTranslationalDistance + rotationalDistance / shortestRotationalDistance);
            TraceOut.Info(elbowPlannerContext, "Combined Ratio: {0}", combined);

            return translationalDistance / shortestTranslationalDistance;
        }
    }
}
